﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Windows;
using NUnit.Framework;

namespace Utility3D
{
    [TestFixture]
    public class PlaneTest
    {
        [Test]
        public void Test()
        {
            var worldModel = DefaultWorldModelGenerator.GetModel();

            var frontSurface = worldModel.DrawingSurfaceChains.First(ds => ds.Name == "Front View");
            var frontSurfaceGlided = worldModel.DrawingSurfaceChains.First(ds => ds.Name == "Front View").Glide(10);
            var topSurface = worldModel.DrawingSurfaceChains.First(ds => ds.Name == "Top View").Glide(50);

            var points = frontSurface.Plane.GetIntersectionPoints(topSurface.Plane).ToList();
            Assert.AreEqual(2, points.Count());
            Assert.AreEqual(50, points.First(p => p.X == 0).Y);
            Assert.AreEqual(50, points.First(p => p.X == 100).Y);

            points = frontSurfaceGlided.Plane.GetIntersectionPoints(topSurface.Plane).ToList();
            Assert.AreEqual(2, points.Count());
            Assert.AreEqual(50, points.First(p => p.X == 0).Y);
            Assert.AreEqual(50, points.First(p => p.X == 100).Y);
        }
    }

    public struct Plane
    {
        public Plane(Point3D basePoint, Vector3D vectorX, Vector3D vectorY)
        {
            _basePoint = basePoint;
            _vectorX = vectorX;
            _vectorY = vectorY;
        }

        private readonly Point3D _basePoint;
        public Point3D BasePoint { get { return _basePoint; } }

        private readonly Vector3D _vectorX;
        public Vector3D VectorX { get { return _vectorX; } }

        private readonly Vector3D _vectorY;
        public Vector3D VectorY { get { return _vectorY; } }

        public Vector3D Normal
        {
            get { return VectorX.CrossProduct(VectorY).Unit(); }
        }

        public IEnumerable<Point> GetIntersectionPoints(Plane plane)
        {
            var intersections = new []
                                    {
                                        GetIntersection(plane.BasePoint, plane.VectorX, true),
                                        GetIntersection(plane.BasePoint, plane.VectorY, true),
                                        GetIntersection(plane.BasePoint + plane.VectorY, plane.VectorX, true),
                                        GetIntersection(plane.BasePoint + plane.VectorX, plane.VectorY, true)
                                    };
            foreach (var point3d in intersections.Where(p => p != null).Select(p => p.Value))
                yield return ProjectTo2D(point3d);
        }

        //TODO: The actual workings of this method are more complicated than they appear at first
        //If mustBeOnLine is false, it only returns if the plane is in the "direction"
        public Point3D? GetIntersection(Point3D pointStart, Vector3D direction, bool mustBeOnLine)
        {
            var normal = direction.Unit();
            var distanceFromPointStartToPlane = (BasePoint - pointStart).DotProduct(Normal)
                     / normal.DotProduct(Normal);

            if (mustBeOnLine && distanceFromPointStartToPlane > direction.Length) return null;
            if (distanceFromPointStartToPlane < 0 || double.IsNaN(distanceFromPointStartToPlane)) return null;

            return pointStart + normal*distanceFromPointStartToPlane;
        }

        public Point3D ToPlaneCoordinates(Point3D point)
        {
            var fromBase = point - BasePoint;
            if (fromBase.Length == 0)
                return new Point3D(0, 0, 0);

            var xUnit = VectorX.Unit();
            var yUnit = VectorY.Unit();
            var zUnit = Normal;

            var matrixA = new double[,]
                              {
                                  {xUnit.X, yUnit.X, zUnit.X},
                                  {xUnit.Y, yUnit.Y, zUnit.Y},
                                  {xUnit.Z, yUnit.Z, zUnit.Z}
                              };
            var matrixAx = new double[,]
                               {
                                  {fromBase.X, yUnit.X, zUnit.X},
                                  {fromBase.Y, yUnit.Y, zUnit.Y},
                                  {fromBase.Z, yUnit.Z, zUnit.Z}
                               };
            var matrixAy = new double[,]
                              {
                                  {xUnit.X, fromBase.X, zUnit.X},
                                  {xUnit.Y, fromBase.Y, zUnit.Y},
                                  {xUnit.Z, fromBase.Z, zUnit.Z}
                              };
            var matrixAz = new double[,]
                              {
                                  {xUnit.X, yUnit.X, fromBase.X},
                                  {xUnit.Y, yUnit.Y, fromBase.Y},
                                  {xUnit.Z, yUnit.Z, fromBase.Z}
                              };
            var detD = matrixA.Determinant();
            var detX = matrixAx.Determinant();
            var detY = matrixAy.Determinant();
            var detZ = matrixAz.Determinant();
            return new Point3D(detX/detD, detY/detD, detZ/detD);
        }

        public Point ProjectTo2D(Point3D point)
        {
            var newPoint = ToPlaneCoordinates(point);
            return new Point(newPoint.X, newPoint.Y);
        }
    }

    public class Line
    {
        public Point Start { get; set; }
        public Point End { get; set; }
    }

    public static class ArrayExtensions
    {
        public static double Determinant(this double[,] array)
        {
            if (array.GetLength(0) != array.GetLength(1)) throw new Exception("Must apply operation to square matrices");
            if (array.GetLength(0) != 3) throw new NotImplementedException("Determinant only implemented for 3 dimensional arrays");

            return array[0, 0]*array[1, 1]*array[2, 2]
                   + array[1, 0]*array[2, 1]*array[0, 2]
                   + array[2, 0]*array[0, 1]*array[1, 2]
                   - array[0, 0]*array[2, 1]*array[1, 2]
                   - array[1, 0]*array[0, 1]*array[2, 2]
                   - array[2, 0]*array[1, 1]*array[0, 2];
        }
    }
}